#pragma once

#include "PhysicsEngine.h"
#include "../FuriousTourist.h"

using namespace physics;

//////////////////////////////////////////////////////////
//						PhysicsEngine					//
//////////////////////////////////////////////////////////
/////////////////////////////////////////////
// constructor()
// 
// initializes the Physics engine
/////////////////////////////////////////////
PhysicsEngine::PhysicsEngine(unsigned maxContacts, unsigned iterations)
:
resolver(iterations),
maxContacts(maxContacts)
{
	// create a new array of Contact objects, based
	// on the max number of contacts we allow
	contacts = new Contact[maxContacts];

	cData.contactArray = contacts;

	globalRegistry.setObjectList(&objects);

	// if the number of iterations is zero (i.e. we
	// didn't ask for a specific number and the default
	// argument took over) then set this boolean which
	// is used later
	if (iterations == 0)
		calculateIterations = true;
	else
		calculateIterations = false;
}

/////////////////////////////////////////////
// destructor()
// 
// deinitializes the Physics engine
/////////////////////////////////////////////
PhysicsEngine::~PhysicsEngine()
{
	delete [] contacts; // delete the dynamically allocated contacts list
}

/////////////////////////////////////////////
// generateContacts()
//
// Generates the ParticleContact objects for Particles
// in contact with one another, which can be later resolved
// by the ContactResolver
/////////////////////////////////////////////
void PhysicsEngine::generateContacts()
{
    // Set up the collision data structure
    cData.reset(maxContacts);
    cData.friction = (real)ENGINE_FRICTION;
    cData.restitution = (real)ENGINE_RESTITUTION;

	// go through every pair of objects, determine what types of objects they are
	// and run the corresponding contact generation function.
	Objects::iterator p = objects.begin();

	while (p != objects.end())
	{
		// primary object type
		PrimitiveType A = (*p)->getPrimitiveType();

		Objects::iterator x = p; // start at the same location as p
		x++; // shift over by one and check every other object

		while (x != objects.end())
		{
			if (!cData.hasMoreContacts())
				return;

			// secondary object type
			PrimitiveType B = (*x)->getPrimitiveType();

			// do something different depending on which types of objects
			// we're deaing with

			// check if both objects are activated. If one is deactivated, skip this step
			if ((*p)->body != NULL && (*x)->body != NULL)
			{
				if (!(*p)->body->getActivated() || !(*x)->body->getActivated())
				{
					x++;
					continue;
				}
			}

			// if neither object has a finite mass (neither can be moved)
			// skip calculations
			/*if (!(*p)->body->hasFiniteMass() && !(*x)->body->hasFiniteMass())
			{
				x++;
				continue;
			}*/

			// needs excessive typecasting to convert from a Primitive to the correct
			// object pointer, then dereference it
			if		(A == PRIMITIVETYPE_COLLISIONSPHERE &&
						B == PRIMITIVETYPE_COLLISIONSPHERE)
			{
				CollisionDetector::sphereAndSphere(*(CollisionSphere*)(*p), *(CollisionSphere*)(*x), &cData);
			}			
			else if (A == PRIMITIVETYPE_COLLISIONSPHERE &&
						B == PRIMITIVETYPE_COLLISIONPLANE)
			{
				CollisionDetector::sphereAndHalfSpace(*(CollisionSphere*)(*p), *(CollisionPlane*)(*x), &cData);
			}			
			else if (A == PRIMITIVETYPE_COLLISIONSPHERE &&
						B == PRIMITIVETYPE_COLLISIONBOX)
					// need to flip the objects around
			{
				CollisionDetector::boxAndSphere(*(CollisionBox*)(*x), *(CollisionSphere*)(*p), &cData);
			}
			else if (A == PRIMITIVETYPE_COLLISIONPLANE &&
						B == PRIMITIVETYPE_COLLISIONSPHERE)
						// need to flip the objects around
			{
				CollisionDetector::sphereAndHalfSpace(*(CollisionSphere*)(*x), *(CollisionPlane*)(*p), &cData);
			}
			else if (A == PRIMITIVETYPE_COLLISIONPLANE &&
						B == PRIMITIVETYPE_COLLISIONBOX)
						// need to flip the objects around
			{
				if ((*x)->body->getActivated())
				{
					CollisionDetector::boxAndHalfSpace(*(CollisionBox*)(*x), *(CollisionPlane*)(*p), &cData);
				}
			}
			else if (A == PRIMITIVETYPE_COLLISIONBOX &&
						B == PRIMITIVETYPE_COLLISIONSPHERE)
			{
				CollisionDetector::boxAndSphere(*(CollisionBox*)(*p), *(CollisionSphere*)(*x), &cData);
			}			
			else if (A == PRIMITIVETYPE_COLLISIONBOX &&
						B == PRIMITIVETYPE_COLLISIONPLANE)
			{		 
				if ((*p)->body->getActivated())
				{
					CollisionDetector::boxAndHalfSpace(*(CollisionBox*)(*p), *(CollisionPlane*)(*x), &cData);
				}
			}
			else if (A == PRIMITIVETYPE_COLLISIONBOX &&
						B == PRIMITIVETYPE_COLLISIONBOX)
			{	
				CollisionDetector::boxAndBox(*(CollisionBox*)(*p), *(CollisionBox*)(*x), &cData);	
			}
			
			// check the next secondary object
			x++;
		}

		// check the next primary object
		p++;
	}

}

/////////////////////////////////////////////
// integrate()
//
// Updates all of the particles in the application
/////////////////////////////////////////////
void PhysicsEngine::integrate(real dt)
{
	Objects::iterator p = objects.begin();
	while (p != objects.end())
	{
		// failsafe to make sure we don't try to clear accumulators for
		// primitive objects with no body e.g. CollisionPlanes
		if ((*p)->body != NULL)
		{
			(*p)->body->zeroZAxis(); // our game plays on a 2D XY plane, so zero everything related to the Z-axis
			(*p)->body->integrate(dt);
			(*p)->calculateInternals();
		}
		p++;
	}

	ParticleEffects::iterator x = particleEffects.begin();
	while (x != particleEffects.end())
	{
		(*x)->Update(dt);
		x++;
	}
}

/////////////////////////////////////////////
// runPhysics()
//
// Processes the physics during this iteration
/////////////////////////////////////////////
void PhysicsEngine::runPhysics(real dt)
{
	
	// apply global forces
	globalRegistry.updateForces(dt);

	// apply the forces
	registry.updateForces(dt);
	
	// integrate the objects
	integrate(dt);

	// generate contacts
	generateContacts();

	// resolve the contacts
	resolver.resolveContacts(cData.contactArray, cData.contactCount, dt);
}
			
/////////////////////////////////////////////
// endFrame()
//
// ends the frame for the new iteration
// by clearing out the force accumulators
/////////////////////////////////////////////
void PhysicsEngine::endFrame()
{
	Objects::iterator p = objects.begin();
	while (p != objects.end())
	{
		// failsafe to make sure we don't try to clear accumulators for
		// primitive objects with no body e.g. CollisionPlanes
		if ((*p)->body != NULL)
		{
			(*p)->body->clearAccumulators();
			(*p)->body->calculateDerivedData();
		}
		p++;
	}

	ParticleEffects::iterator x = particleEffects.begin();
	while (x != particleEffects.end())
	{
		(*x)->EndFrame();
		x++;
	}
	
}

/////////////////////////////////////////////
// update()
//
// what the physics engine does each cycle
/////////////////////////////////////////////
void PhysicsEngine::update(real dt)
{
	// apply forces etc.
	runPhysics(dt);

	// clear accumulators
	endFrame();
}

/////////////////////////////////////////////
// getParticles()
//
// gets the list of particles
/////////////////////////////////////////////
PhysicsEngine::Objects& PhysicsEngine::getObjects()
{
	return objects;
}

/////////////////////////////////////////////
// getContactGenerators()
//
// gets the list of contact generators
/////////////////////////////////////////////
PhysicsEngine::ContactGenerators& PhysicsEngine::getContactGenerators()
{
	return contactGenerators;
}

/////////////////////////////////////////////
// getForceRegistry()
//
// gets the force registry
/////////////////////////////////////////////
ForceRegistry& PhysicsEngine::getForceRegistry()
{
	return registry;
}

/////////////////////////////////////////////
// getGlobalRegistry()
//
// gets the global force registry
/////////////////////////////////////////////
GlobalForceRegistry& PhysicsEngine::getGlobalForceRegistry()
{
	return globalRegistry;
}

/////////////////////////////////////////////
// AddObject()
//
// adds the object to the list of collionPrimitives
/////////////////////////////////////////////
void PhysicsEngine::AddObject(CollisionPrimitive* object)
{
	objects.push_back(object);
}

/////////////////////////////////////////////
// RemoveObject()
//
// removes the object to the list of collionPrimitives
/////////////////////////////////////////////
void PhysicsEngine::RemoveObject(RigidBody* object)
{
	Objects::iterator i = objects.begin();
	while (i != objects.end())
	{
		if ((*i)->body == object)
		{
			registry.remove((*i)->body);
			delete *i;
			objects.erase(i);
			return;
		}
		else
		{
			i++;
		}
	}
}

/////////////////////////////////////////////
// RemoveAllObjects()
//
// removes all objects from the list of collionPrimitives
/////////////////////////////////////////////
void PhysicsEngine::RemoveAllObjects()
{
	Objects::iterator i = objects.begin();
	while (i != objects.end())
	{
		registry.remove((*i)->body);
		delete *i;
		i = objects.erase(i);
	}
}

/////////////////////////////////////////////
// HasCollided()
//
// Checks if the given object has a collision to resolve during this iteration
/////////////////////////////////////////////
bool PhysicsEngine::HasCollided(RigidBody* object)
{
	Contact *c = cData.contactArray;
	for (unsigned int i = 0; i < cData.contactCount; i++)
	{
		// if either object is this object, return true
		if (c[i].objects[0] == object || c[i].objects[1] == object)
			return true;

	}
	// if we didn't find it, return false
	return false;
}


/////////////////////////////////////////////
// AddParticleEffect()
//
// adds the particle effect to the list of particle effects
/////////////////////////////////////////////
void PhysicsEngine::AddParticleEffect(ParticleEffect* particleEffect)
{
	particleEffects.push_back(particleEffect);
}

/////////////////////////////////////////////
// RemoveParticleEffects()
//
// Removes the particle Effect from the list of Particle Effects
/////////////////////////////////////////////
void PhysicsEngine::RemoveParticleEffect(ParticleEffect* particleEffect)
{
	ParticleEffects::iterator i = particleEffects.begin();
	while (i != particleEffects.end())
	{
		if ((*i) == particleEffect)
		{
			delete *i;
			particleEffects.erase(i);
			return;
		}
		else
		{
			i++;
		}
	}
}

/////////////////////////////////////////////
// RemoveAllParticleEffects()
//
// Removes all particles Effects from the list of particle Effects
/////////////////////////////////////////////
void PhysicsEngine::RemoveAllParticleEffects()
{
	ParticleEffects::iterator i = particleEffects.begin();
	while (i != particleEffects.end())
	{
		delete *i;
		i = particleEffects.erase(i);
	}
}